#include "transform.h"
#include "matrixcom.h"
#include <cmath>

namespace CoreBase
{
   Transform::Transform(float tx, float ty, float tz, float h, float p, float r)
   {
      set(tx, ty, tz, h, p, r);
   }

   Transform::Transform(const Transform& that)
   {
      _ransform = that._ransform;
   }

   Transform::~Transform()
   {}

   void Transform::getRow(unsigned index, osg::Vec3& row) const
   {
      row = MatrixCom::getRow3(_ransform, int(index));
   }

   void Transform::setRow(unsigned index, const osg::Vec3& row)
   {
      MatrixCom::setRow(_ransform, row, int(index));
   }

   void Transform::getRow(unsigned index, osg::Vec4& row) const
   {
      row = MatrixCom::getRow4(_ransform, int(index));
   }

   void Transform::setRow(unsigned index, const osg::Vec4& row)
   {
      MatrixCom::setRow(_ransform, row, int(index));
   }

   void Transform::set(float tx, float ty, float tz, float h, float p, float r)
   {
      osg::Vec3 xyz(tx, ty, tz);
      osg::Vec3 hpr(h, p, r);
      set(xyz, hpr);
   }

   void Transform::set(const osg::Vec3& xyz, const osg::Matrix& rotation)
   {
      _ransform.set(rotation);
      _ransform.setTrans(xyz);
   }

   void Transform::set(const osg::Vec3& xyz, const osg::Quat& quat)
   {
      setTranslation(xyz);
      setRotation(quat);
   }

   void Transform::set(const osg::Vec3& xyz, const osg::Vec3& hpr)
   {
      osg::Matrix rotation;
      MatrixCom::hprToMatrix(rotation, hpr);
      set(xyz, rotation);
   }

   void Transform::set(const osg::Matrix& mat)
   {
      _ransform = mat;
   }

   void Transform::setRotation(const osg::Vec3& hpr)
   {
      osg::Matrix rotation;
      MatrixCom::hprToMatrix(rotation, hpr);
      setRotation( rotation );
   }

   void Transform::setRotation(const osg::Matrix& rotation)
   {
      osg::Vec3 trans;
	   osg::Matrix oldrotation, matScale;
      MatrixCom::decompose(_ransform, oldrotation, matScale, trans);
	  oldrotation.set(rotation);
      oldrotation.setTrans(trans);
      _ransform = matScale*oldrotation;
   }

   void Transform::get(float& tx, float& ty, float& tz, float& h, float& p, float& r) const
   {
      getTranslation(tx, ty, tz);
      getRotation(h, p, r );
   }

   void Transform::get( osg::Vec3& xyz, osg::Matrix& rotation) const
   {
      getTranslation(xyz);
      getRotation(rotation);
   }

   void Transform::get(osg::Matrix& matrix) const
   {
      matrix = _ransform;
   }

   void Transform::get(osg::Vec3& xyz, osg::Quat& quat) const
   {
      getTranslation(xyz);
      getRotation(quat);
   }

   void Transform::get(osg::Vec3& xyz, osg::Vec3& hpr) const
   {
      getTranslation(xyz);
      getRotation(hpr);
   }

   void Transform::getTranslation(float& tx, float& ty, float& tz) const
   {
      osg::Vec3f vec3;
      getTranslation(vec3);
      tx = vec3.x();
      ty = vec3.y();
      tz = vec3.z();
   }

   void Transform::move(const osg::Vec3f& distance)
   {
      _ransform.setTrans(_ransform.getTrans() + distance);
   }

   void Transform::move(const osg::Vec3d& distance)
   {
      _ransform.setTrans(_ransform.getTrans() + distance);
   }

   void Transform::getRotation(float& h, float& p, float& r) const
   {
      osg::Vec3 hpr;
      MatrixCom::matrixToHpr(hpr, _ransform);
      h = hpr[0];
      p = hpr[1];
      r = hpr[2];
   }

   void Transform::getRotation(osg::Vec3& hpr) const
   {
      MatrixCom::matrixToHpr(hpr, _ransform);
   }

   void Transform::getRotation(osg::Matrix& rotation) const
   {
      rotation.set(_ransform);
      rotation.setTrans(0.0, 0.0, 0.0);
   }

   template <typename VecType>
   static void temRescale(const VecType& scale, osg::Matrix& toScale)
   {
      osg::Matrix rotation, matScale;
      osg::Vec3 translation;
      MatrixCom::decompose(toScale, rotation, matScale, translation);
      matScale.makeScale(scale);
      rotation.setTrans(translation);
      toScale = matScale*rotation;
   }

   template <typename VecType>
   static void temCalcScale(VecType& scale, const osg::Matrix& transform)
   {
      osg::Matrix rotation, matScale;
      osg::Vec3 translation;
      MatrixCom::decompose(transform, rotation, matScale, translation);
      scale[0] = matScale(0,0);
      scale[1] = matScale(1,1);
      scale[2] = matScale(2,2);
   }

   void Transform::calcScale(osg::Vec3f& scale) const
   {
      temCalcScale(scale, _ransform);
   }

   void Transform::calcScale(osg::Vec3d& scale) const
   {
      temCalcScale(scale, _ransform);
   }

   void Transform::rescale(const osg::Vec3d& scale)
   {
      temRescale(scale, _ransform);
   }

   void Transform::rescale(const osg::Vec3f& scale)
   {
      temRescale(scale, _ransform);
   }

   double Transform::calcDistance(const Transform& xform)
   {
      return sqrt(calcDistanceSquared(xform));
   }

   double Transform::calcDistanceSquared(const Transform& xform)
   {
      osg::Vec3 positionOne, positionTwo;
      getTranslation(positionOne);
      xform.getTranslation(positionTwo);
      return (((positionOne[0]-positionTwo[0]) * (positionOne[0]-positionTwo[0])) +
              ((positionOne[1]-positionTwo[1]) * (positionOne[1]-positionTwo[1])) +
              ((positionOne[2]-positionTwo[2]) * (positionOne[2]-positionTwo[2])));
   }

   void Transform::set(const osg::Vec3& xyz, const osg::Vec3& lookatxyz, const osg::Vec3& upvec)
   {
      osg::Vec3 x,y,z;
      y = lookatxyz - xyz;
      z = upvec;
      x = y ^ z;
      z = x ^ y;
      x.normalize();
      y.normalize();
      z.normalize();
      MatrixCom::setRow(_ransform, x, 0);
      MatrixCom::setRow(_ransform, y, 1);
      MatrixCom::setRow(_ransform, z, 2);
      MatrixCom::setRow(_ransform, xyz, 3);
   }

   void Transform::set(float posx, float posy, float posz,
                       float lookatx, float lookaty, float lookatz,
                       float upvecx, float upvecy, float upvecz)
   {
      osg::Vec3 xyz(posx, posy, posz);
      osg::Vec3 lookAt(lookatx, lookaty, lookatz);
      osg::Vec3 upvec(upvecx, upvecy, upvecz);
      set(xyz, lookAt, upvec);
   }

   void Transform::getOrientation(osg::Vec3& right, osg::Vec3& up, osg::Vec3& forward)
   {
      right   = MatrixCom::getRow3(_ransform, 0);
      forward = MatrixCom::getRow3(_ransform, 1);
      up      = MatrixCom::getRow3(_ransform, 2);
   }

   bool Transform::epsilonEquals(const Transform& transform, float epsilon) const
   {
      for(int i = 0; i < 4; i++)
      {
         for(int j = 0; j < 4; j++)
         {
            if(!MatrixCom::equivalent(_ransform(i,j), transform._ransform(i,j), osg::Matrix::value_type(epsilon)))
            {
               return false;
            }
         }
      }
      return true;
   }

   Transform & Transform::operator=(const Transform & rhs)
   {
      if (this == &rhs) return *this;
      _ransform = rhs._ransform;
      return *this;
   }

   bool Transform::operator==(const Transform & rhs)
   {
      if (this == &rhs) return true;
      return epsilonEquals(rhs, 0.0f);
   }
}